#include "S_MOTOR.h"
#include "math.h"
#include "S_PID.h"
#include "S_SYSTICK.h"
#include "S_ENCODER.h"
TIM_HandleTypeDef *motortim[4] = {&htim3, &htim3, &htim3, &htim3};
uint32_t motortimch[4] = {TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4};

void MOTORInit(uint8_t status)
{
	for (uint8_t i = 0; i < 4; i++)
		if (status & (1 << (3 - i)))
			HAL_TIM_PWM_Start(motortim[i], motortimch[i]);
}

GPIO_TypeDef *motorctrlA[4] = {M1AO_GPIO_Port, M2AO_GPIO_Port};
GPIO_TypeDef *motorctrlB[4] = {M1BO_GPIO_Port, M2BO_GPIO_Port};
uint16_t motorctrlAPin[4] = {M1AO_Pin, M2AO_Pin};
uint16_t motorctrlBPin[4] = {M1BO_Pin, M2BO_Pin};

/**
 * @brief 用于控制电机
 * 
 * @param pwm 范围是-100~100
 * @param motornum 电机编号1-4
 */
void MOTORCtrl(float pwm, uint8_t motornum)
{
	motornum -= 1;
	if (pwm < 0)
	{
		HAL_GPIO_WritePin(motorctrlA[motornum], motorctrlAPin[motornum], 0);
		HAL_GPIO_WritePin(motorctrlB[motornum], motorctrlBPin[motornum], 1);
		pwm = -pwm;
	}
	else
	{
		HAL_GPIO_WritePin(motorctrlA[motornum], motorctrlAPin[motornum], 1);
		HAL_GPIO_WritePin(motorctrlB[motornum], motorctrlBPin[motornum], 0);
	}
	switch (motornum)
	{
	case 0:
		htim3.Instance->CCR1 = htim3.Instance->ARR * pwm / 100;
		break;
	case 1:
		htim3.Instance->CCR2 = htim3.Instance->ARR * pwm / 100;
		break;

	default:
		break;
	}
}

PID_t motor1;
PID_t pos1;
PID_t motor2;
PID_t pos2;
void MOTORPosCtrlInit(void)
{

	PIDInit(&pos1, 0.01, 50.0f, 20.0, 1.5);
	PIDConfig(&pos1, 20.0f, 0.001f, 0.06f);
	PIDInit(&motor1, 0.01, 50.0f, 2.0f, 80.0f);
	PIDConfig(&motor1, 200, 400, 0.01);

	PIDInit(&pos2, 0.01, 50.0f, 20.0, 1.5);
	PIDConfig(&pos2, 20.0f, 0.001f, 0.06f);
	PIDInit(&motor2, 0.01, 50.0f, 2.0f, 80.0f);
	PIDConfig(&motor2, 200, 400, 0.01);

	PIDSetTarget(&pos1, 0);
	PIDSetTarget(&motor1, 0);

	PIDSetTarget(&pos2, 0);
	PIDSetTarget(&motor2, 0);
}

/**
 * @brief 电机位置环
 * 
 * @param pos 
 * @param motornum 
 */
void MOTORPosCtrl(float position1, float position2)
{
	pos1.target = -position1;
	pos2.target = -position2;
	float speed1, speed2;
	int32_t val1 = -GetEncoderVal(0);
	int32_t val2 = GetEncoderVal(1);

	speed1 = (float)val1 * 100.0f / 5065.0f;
	speed2 = (float)val2 * 100.0f / 5065.0f;

	//电机1的位置环////////////////////////////////////////////
	pos1.current += (float)val1 / 5065.0f;
	motor1.current = speed1;
	PID_Calc(&pos1);
	motor1.target = pos1.OUT;
	PID_Calc(&motor1);
	MOTORCtrl(motor1.OUT, 1);
	//电机2的位置环/////////////////////////////////////////////
	pos2.current += (float)val2 / 5065.0f;
	motor2.current = speed2;
	PID_Calc(&pos2);
	motor2.target = pos2.OUT;
	PID_Calc(&motor2);
	MOTORCtrl(motor2.OUT, 2);
}